#ifndef _CVADDON_DRAW_H
#define _CVADDON_DRAW_H

////////////////////////////////////////////////////////////
//                 CvAddon Draw Functions
////////////////////////////////////////////////////////////
// By Wai Ho Li
////////////////////////////////////////////////////////////
// Functions that draw onto IplImages. 
////////////////////////////////////////////////////////////
// Usage:
// ---
// See individual functions for detailed usage instructions.
////////////////////////////////////////////////////////////
// TODO:
// ---
// - Make all functions handle ROI
////////////////////////////////////////////////////////////


#include <cv.h>
#include "cvaddon_math.h"

// OpenCv doesnt have a function to draw Rectangles. Go figure...
inline void cvAddonDrawRectangle(CvArr *dst, const CvRect& rect
	, const CvScalar& colour, const int& thickness=1
	, const int& linetype=8)
{
	cvRectangle(dst
		, cvPoint(rect.x, rect.y)
		, cvPoint(rect.x + rect.width - 1, rect.y + rect.height - 1)
		, colour
		, thickness
		, linetype);
}

// Draws pixels on an image using <color>. 
template <typename T>
inline void cvAddonDrawPixels(IplImage *dst, CvPoint *pixels
	, const int& numPixels, const CvScalar &color)
{
	int i, j;
	int x,y;
	int x0, x1, y0, y1;
	CvRect rect;
	unsigned int widthStep;
	unsigned int channels;

	CV_FUNCNAME("cvAddonDrawPixels");

	__BEGIN__;

	if(dst == NULL) CV_ERROR( CV_StsBadArg, "Null Pointer Destination <dst>" );
	if(pixels == NULL) CV_ERROR( CV_StsBadArg, "Null Pointer Pixels Data <pixels>" );

 	rect = cvGetImageROI(dst);
	widthStep = dst->widthStep;
	channels = dst->nChannels;

	x0 = rect.x;
	x1 = rect.x + rect.width;
	y0 = rect.y;
	y1 = rect.y + rect.height;
	for(i = 0; i < numPixels; ++i)
	{
		x = pixels[i].x;
		y = pixels[i].y;

		if(x >= x0 && x < x1 && y >= y0 && y < y1) {
			T *val = (T*)(dst->imageData + widthStep*y) + x*channels;
			for(j = 0; j < channels; ++j) {
				val[j] = (T)color.val[j]; 
			}
		}
	}

	__END__;
}



// Draws a straight line defined in polar form relative to the 
// center of the image <dst>
inline void cvAddonDrawPolarLine(IplImage *dst, const float &r, const float &theta
	, const CvScalar &color, const int &thickness = 1, const int& lineType = 8)
{
	CvPoint2D32f xrPt, p0, p1;

	CV_FUNCNAME("cvAddonDrawPolarLine");

	__BEGIN__;

	if(dst == NULL) CV_ERROR( CV_StsBadArg, "Null Pointer Destination <dst>" );

	cvAddonFindPolarLineEndPoints(cvGetSize(dst), r, theta, xrPt, p0, p1);

	cvLine(dst, cvPointFrom32f(xrPt), cvPointFrom32f(p0), color, thickness, lineType);
	cvLine(dst, cvPointFrom32f(xrPt), cvPointFrom32f(p1), color, thickness, lineType);

//#ifdef _DEBUG
//	// Draw end points
//	cvLine(dst, cvPointFrom32f(p0), cvPointFrom32f(p0), color, thickness*3, lineType);
//	cvLine(dst, cvPointFrom32f(p1), cvPointFrom32f(p1), color, thickness*3, lineType);
//
//	// Draw pivot point (perpendicular to line, connecting line to image center)
//	cvLine(dst, cvPointFrom32f(xrPt), cvPointFrom32f(xrPt), color, thickness*3, CV_AA, lineType);
//#endif

	__END__;
}


// Fills a fixed-width border around an image with a constant color. Useful for zeroing
// edge noise generated by noisy camera input.
inline void cvAddonFillBorder(IplImage *dst, const int &width, const CvScalar &color)
{
	int bw;
	int w,h;	//Image dimensions

	CV_FUNCNAME("cvAddonFillBorder");

	__BEGIN__;

	if(dst == NULL) CV_ERROR( CV_StsBadArg, "Null Pointer Destination <dst>" );
	if(width <= 0) bw = 10;
	else bw = width;

	w = dst->width;
	h = dst->height;
	
	cvRectangle(dst, cvPoint(0,0), cvPoint(bw-1, h-1), color, CV_FILLED);		// Left border
	cvRectangle(dst, cvPoint(w-bw,0), cvPoint(w-1, h-1), color, CV_FILLED);		// Right Border
	cvRectangle(dst, cvPoint(bw,0), cvPoint(w-bw-1, bw-1), color, CV_FILLED);	// "Top" border (y ~= 0)
	cvRectangle(dst, cvPoint(bw,h-bw), cvPoint(w-bw-1, h-1), color, CV_FILLED);	// "Bottom" border

	__END__;
}

// Draws an arrow from start to end, with the arrow head sized based on
// <arrowLenRatio>, which should be between 0 and 1.0f
inline void cvAddonDrawArrow(IplImage *dst
	, const CvPoint& start, const CvPoint& end, const CvScalar colour
	, const float& arrowLenRatio = 0.2f)
{
	// Marking start and end locations with circles
	cvCircle(dst, start, 3, colour, CV_FILLED, CV_AA);

	// Drawing arrow
	CvPoint2D32f vec0 = cvPoint2D32f(end.x-start.x, end.y-start.y);
	CvPoint2D32f vec1 = cvPoint2D32f(-vec0.y, vec0.x);
	CvPoint2D32f vec2 = cvPoint2D32f(vec0.y, -vec0.x);

	CvPoint poly[3];
	CvPoint mid = cvPoint( start.x + cvRound( (float)(end.x - start.x)*(1.0f - arrowLenRatio) )
		, start.y + cvRound( (float)(end.y - start.y)*(1.0f - arrowLenRatio) ) );
	poly[0] = cvPoint( cvRound((float)mid.x + vec1.x * arrowLenRatio), cvRound((float)mid.y + vec1.y * arrowLenRatio));
	poly[1] = cvPoint( cvRound((float)mid.x + vec2.x * arrowLenRatio), cvRound((float)mid.y + vec2.y * arrowLenRatio));
	poly[2] = end;

	cvFillConvexPoly( dst, poly, 3, colour, CV_AA);		// Arrowhead
	cvLine(dst, start, mid, colour, 2, CV_AA);		// Arrowline						   
}

#endif
